﻿using System;
using Microsoft.Xna.Framework;
using XNAGameEngine.DataStructures;

namespace XNAGameEngine.Pathfinding
{
    /// <summary>
    /// Implmentation of a 2d pathfinding system using A* algorithm
    /// </summary>
    public class PathfindingSystem
    {
        #region local fields

        /// <summary>
        /// have the local variables been initialized
        /// </summary>
        private bool _isInitialized;

        /// <summary>
        /// <para>Divide the screen into this number of columns</para>
        /// <para>Minimum is 10, Maximum is 500</para>
        /// </summary>
        private int _numColumns;

        /// <summary>
        /// <para>Divide the screen into this number of rows</para>
        /// <para>Minimum is 10, Maximum is 500</para>
        /// </summary>
        private int _numRows;

        /// <summary>
        /// <para>The cost to move vertical and horizontal</para>
        /// <para>Must be a positive number</para>
        /// </summary>
        private double _adjacentMoveCost;

        /// <summary>
        /// <para>The cost to move diagonally</para>
        /// <para>set to 0 to disable diagonal movement</para>
        /// </summary>
        private double _diagonalMoveCost;

        /// <summary>
        /// <para>Array containing the map layout, determining whether or not a
        ///     specific cell is passable.</para>
        /// <para>Value should be true if it is passable, or false if impassable
        /// </para>
        /// </summary>
        private bool[,] _passableCellMap;

        /// <summary>
        /// Stores the possible location of child cells
        /// </summary>
        private bool[,] _childLocations;

        /// <summary>
        /// Array containing all of the cells in the map
        /// </summary>
        private PathfindingCell[,] _cellMap;

        /// <summary>
        /// The current cell being evaluted
        /// </summary>
        private PathfindingCell _currentCell;

        /// <summary>
        /// The temporary cell value used to process the child cells
        /// </summary>
        private PathfindingCell _tempCell;

        /// <summary>
        /// The temporary cell location used to process child cells
        /// </summary>
        private Point _tempLocation;

        /// <summary>
        /// The current location in the array being evaluated
        /// </summary>
        private Point _currentLocation;

        /// <summary>
        /// The open list of nodes
        /// </summary>
        private Heap<PathfindingCell> _openList;



        #endregion local fields

        #region constructors

        /// <summary>
        /// Construct a new pathfinding manager, initialize local values
        /// </summary>
        /// <param name="numColumns">number of rows, minimum 10</param>
        /// <param name="numRows">number of columns, minimum 10</param>
        /// <param name="adjMoveCost">adjacent move cost, must be positive</param>
        /// <param name="diagMoveCost">diagonal move cost. must be positive, or
        ///     0 to disable diagonal movement, or negative to have it 
        ///     calculated to be in proportion to the adjacent move cost</param>
        /// <param name="passableCellMap">array containing the true/false values to 
        ///     determine if a cell is passable. true for passable, false for 
        ///     impassable</param>
        public PathfindingSystem(int numColumns, int numRows, double adjMoveCost,
            double diagMoveCost, bool[,] passableCellMap)
        {
            _openList = new Heap<PathfindingCell>();
            _isInitialized = false;
            _childLocations = new bool[3, 3];

            //ensure minimum value of 10
            _numColumns = numColumns >= 10 ? numColumns : 10;
            _numRows = numRows >= 10 ? numRows : 10;

            //ensure minimum value of 1
            _adjacentMoveCost = adjMoveCost > 0 ? adjMoveCost : 1;

            //calculate the diagonal movement cost
            _diagonalMoveCost = diagMoveCost >= 0 ? diagMoveCost
                                        : _adjacentMoveCost * 1.4142;

            _cellMap = new PathfindingCell[_numColumns, _numRows];

            //if the map array is the correct size then copy it
            if (passableCellMap.Length == _numColumns * _numRows)
            {
                _passableCellMap = passableCellMap;
                _isInitialized = true;
                return;
            }

            //else throw an exception
            throw new Exception("passableCellMap array passed in was not of the " +
                "correct dimensions");
        }

        #endregion constructors

        #region local methods

        /// <summary>
        /// Initialize the pathfinding manager. This should be called before
        ///     each pathfinding search
        /// </summary>
        public void Initialize()
        {
            //remove everything from the open list
            while (_openList.Count > 0)
                _openList.Pop();



            _currentLocation.X = 0;
            _currentLocation.Y = 0;

            //initialize the cell map
            for (int x = 0; x < _numColumns; x++)
                for (int y = 0; y < _numRows; y++)
                {
                    _cellMap[x, y]._gCost = 0x7fffffff;
                    _cellMap[x, y]._previousCell = _currentLocation;
                }
        }

        /// <summary>
        /// Returns the heuristic cost between the two points. Works with or
        ///     without diagonal movement
        /// </summary>
        /// <param name="p1">position 1 to evaluate</param>
        /// <param name="p2">position 2 to evaluate</param>
        /// <returns> heuristic cost between the two points passed in</returns>
        private double GetHCost(Point p1, Point p2)
        {
            //return manhattan distance if diagonal movement is not allowed
            if (_diagonalMoveCost == 0)
                return _adjacentMoveCost *
                    (Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y));

            //else return diagonal distance
            double t1 = Math.Min(Math.Abs(p1.X - p2.X), Math.Abs(p1.Y - p2.Y));
            return (_diagonalMoveCost * t1) + (_adjacentMoveCost *
                ((Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y)) - 2 * t1));
        }

        #endregion local methods

        /// <summary>
        /// Returns an array of Point objects which contain the optimal path
        ///     from the start to the finish. An array of 0 elements is returned
        ///     if no path can be found
        /// </summary>
        /// <param name="startLocation">location to start searching</param>
        /// <param name="endLocation">location to end searching</param>
        /// <returns>the array containing the coordinates of the optimal path
        /// </returns>
        public Point[] FindPath(Point startLocation, Point endLocation)
        {
            if (!_isInitialized)
                throw new Exception(
                    "search values have not been set. you must first call " +
                    "the SetValues() method to initialize the manager");

            //initialize the values to start a new search
            Initialize();

            
            _cellMap[startLocation.X, startLocation.Y]._gCost = 0;
            _cellMap[startLocation.X, startLocation.Y]._hCost =
                GetHCost(startLocation, endLocation);
            _cellMap[startLocation.X, startLocation.Y]._previousCell =
                startLocation;
            _currentCell = _cellMap[startLocation.X, startLocation.Y];
            _currentLocation = startLocation;

            //for loop variables, optimizations
            int x, y;

            //loop until the ending location is found
            while (_currentLocation != endLocation)
            {
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++)
                    _childLocations[i,j] = true;
                
                //current location is not a valid move
                _childLocations[1, 1] = false;

                //updates the array to make sure it only contains valid child cells
                if (_currentLocation.X == 0)
                    _childLocations[0, 0] = _childLocations[0, 1] =
                        _childLocations[0, 2] = false;
                else if (_currentLocation.X == _numColumns)
                    _childLocations[2, 0] = _childLocations[2, 1] =
                        _childLocations[2, 2] = false;
                if (_currentLocation.Y == 0)
                    _childLocations[0, 0] = _childLocations[1, 0] =
                        _childLocations[2, 0] = false;
                else if (_currentLocation.Y == _numColumns)
                    _childLocations[0, 2] = _childLocations[1, 2] =
                        _childLocations[2, 2] = false;

                //remove the diagonal child cells if no diagonal movements allowed
                if (_diagonalMoveCost == 0)
                    _childLocations[0, 0] = _childLocations[2, 0] =
                        _childLocations[0, 2] = _childLocations[2, 2] = false;

                //iterated through each of the valid child cell moves
                for (x = -1; x < 2; x++)
                    for (y = -1; y < 2; y++)
                    {
                        _tempLocation.X = _currentLocation.X + x;
                        _tempLocation.Y = _currentLocation.Y + y;

                        //if not a valid child then skip this one
                        if (!_childLocations[x + 1, y + 1] || _currentLocation.Y + y >= _numRows || _currentLocation.X + x >= _numColumns)
                            continue;
                        
                        if(!_passableCellMap[_tempLocation.X, _tempLocation.Y])
                            continue;

                        _tempCell = _cellMap[_tempLocation.X, _tempLocation.Y];

                        //if the childs gCost is already less, then skip it
                        //because there is already a shorter path to it
                        if (_tempCell._gCost <= _currentCell._gCost + (Math.Abs(x - y) == 1 ?
                            _adjacentMoveCost : _diagonalMoveCost))
                            continue;

                        //set the gCost
                        _tempCell._gCost = _currentCell._gCost +
                            (Math.Abs(x - y) == 1 ?
                            _adjacentMoveCost : _diagonalMoveCost);

                        //set the hCost
                        _tempCell._hCost = GetHCost(_tempLocation, endLocation);
                        _tempCell._location.X = _tempLocation.X;
                        _tempCell._location.Y = _tempLocation.Y;
                        _tempCell._previousCell.X = _currentLocation.X;
                        _tempCell._previousCell.Y = _currentLocation.Y;

                        //put it back in the array and onto the open list
                        _cellMap[_tempLocation.X, _tempLocation.Y] = _tempCell;
                        _openList.Push(_tempCell);
                    }


                //if nothing left on the open list then a path does not exist, return null
                if (_openList.Count == 0)
                    return null;
                
                //grab next item from the open list evaluate from that position
                _currentCell = _openList.Pop();
                _currentLocation = _currentCell._location;
            }

            //calculate the size of the array needed for the return result
            x = 0;
            while (_tempLocation != startLocation)
            {
                _tempLocation = _currentCell._previousCell;
                _currentCell = _cellMap[_tempLocation.X, _tempLocation.Y];
                x++;
            }

            _currentCell = _cellMap[endLocation.X, endLocation.Y];
            Point[] _pathList = new Point[x--];

            //build the array containing the path
            while (x >= 0)
            {
                _pathList[x] = _currentCell._location;
                _currentCell = _cellMap[_currentCell._previousCell.X,
                        _currentCell._previousCell.Y];
                x--;
            }
            
            //return the optimal path to the target
            return _pathList;
        }

    }
}
